/* 
 * File:   surfacealigndomain.cc
 * Author: boyan
 * 
 * Created on April 1, 2009, 2:47 PM
 */

#include <iostream>


#include <vector>
#include <map>
#include <set>
#include <utility>
#include <algorithm>
#include <fstream>
#include <assert.h>

#include <Vector3D.hpp>
#include <Matrix3D.hpp>
#include <MsqVertex.hpp>
#include <MsqError.hpp>

#include "SurfaceAlignDomain.h"
#include "grid3d.h"
#include "cxx_misc.h"

using Mesquite::Matrix3D;

SurfaceAlignDomain::SurfaceAlignDomain(Mesh& surf)
{
    init(surf);
    build_edges();
    compute_balls();
};

LevelsetCode SurfaceAlignDomain::levelset_code(Mesh::EntityHandle entity_handle,
    const Mesquite::Vector3D &pos) const
{
    MsqError err;
    Vector3D proj, norm;
    closest_point(entity_handle, pos, proj, norm, err);
    if(Vector3D::distance_between(pos, proj)<1e-6)
        return ls_onsurf;
    if((pos-proj)%norm>0)
        return ls_outside; // positive outside
    else
        return ls_inside;
}

void SurfaceAlignDomain::snap_to(Mesh::EntityHandle entity_handle,
    Vector3D &coordinate) const
{
    MsqError err;
    Vector3D pos=coordinate, nor;
    closest_point(entity_handle, pos, coordinate, nor, err);
}

void SurfaceAlignDomain::normal_at(Mesh::EntityHandle entity_handle,
    Vector3D &coordinate) const
{
    MsqError err;
    Vector3D pos=coordinate, close;
    closest_point(entity_handle, pos, close, coordinate, err);
}

void SurfaceAlignDomain::normal_at(const Mesh::EntityHandle* handles,
    Vector3D coordinates[], unsigned count, MsqError& err) const
{
    (void)err;
    for(unsigned ind=0; ind<count; ind++)
        normal_at(handles[ind], coordinates[ind]);
}

void SurfaceAlignDomain::closest_point(Mesh::EntityHandle handle,
    const Vector3D& position, Vector3D& closest, Vector3D& normal,
    MsqError& err) const
{
    (void)err;
    (void)handle;
    // Find the nearest node
    double dist, MinDist=1e99; // large number
    SurfNode N=nodes.operator [](0);
    for(size_t idx=0; idx<nodes.size(); idx++)
    {
        dist=Vector3D::distance_between(position, nodes[idx]);
        if(dist<MinDist)
        {
            MinDist=dist;
            N=nodes[idx];
        }
        if(MinDist<1e-12)
        { // position is a node
            closest=N;
            normal=N.GetSurfNormal();
            return;
        }
    }
    normal=N.GetSurfNormal(position);
    closest=N.GetProjection(position);
}



/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

void SurfaceAlignDomain::init(Mesh &surf)
{
    MsqError err;
    std::vector<Mesh::VertexHandle> vert_handles;
    std::vector<Mesh::ElementHandle> elem_handles;
    std::map<Mesh::VertexHandle, size_t> map; // maps vertex handles to indices

    // get nodes
    surf.get_all_vertices(vert_handles, err);
    nodes.clear();
    nodes.reserve(vert_handles.size());
    for(size_t idx=0; idx<vert_handles.size(); idx++)
    {
        Mesquite::MsqVertex coord;
        surf.vertices_get_coordinates(&vert_handles[idx], &coord, (size_t)1, err);
        nodes.push_back(SurfNode(coord));
        map.insert(map.end(), std::make_pair(vert_handles[idx], idx));
    }

    // get faces
    surf.get_all_elements(elem_handles, err);
    faces.clear();
    faces.reserve(elem_handles.size());
    for(size_t idx=0; idx<elem_handles.size(); idx++)
    {
        // the following 3-line check may be redundant
        {
            Mesquite::EntityTopology topo;
            surf.elements_get_topologies(&elem_handles[idx], &topo, 1, err);
            assert(topo==Mesquite::TRIANGLE);
        }
        // get vertices of face idx
        std::vector<Mesh::VertexHandle> local_vertices;
        std::vector<size_t> offsets;
        surf.elements_get_attached_vertices(&elem_handles[idx], 1,
            local_vertices, offsets, err);
        if(err)
        {
            std::cout<<err.error_message()<<std::endl;
            exit(EXIT_FAILURE);
        }
        // the following check is redundant
        assert(offsets[1]==3);
        // map vertex handles to local node indices
        size_t nodes[3];
        for(size_t j=0; j<3; j++)
        {
            std::map<Mesh::VertexHandle, size_t>::iterator found=
                map.find(local_vertices[j]);
            assert(found!=map.end());
            nodes[j]=(*found).second;
        }
        // add face
        faces.push_back(SurfFace(nodes));
    }
}

struct __edge
{
    size_t nodes[2];
    size_t face;

    __edge(size_t n0, size_t n1, size_t f) : face(f)
    {
        nodes[0]=n0;
        nodes[1]=n1;
        sort2(nodes[0], nodes[1]);
    }

    bool operator==(const __edge&rhs) const
    {
        return nodes[0]==rhs.nodes[0]&&nodes[1]==rhs.nodes[1];
    }

    bool operator<(const __edge&rhs) const
    {
        return nodes[0]<rhs.nodes[0]
            ||(nodes[0]==rhs.nodes[0]&&nodes[1]<rhs.nodes[1]);
    }
};

void SurfaceAlignDomain::build_edges()
{
    std::vector<__edge> __edges;

    // add three edges for each face
    __edges.reserve(3*faces.size());
    for(size_t idx=0; idx<faces.size(); idx++)
    {
        SurfFace F=faces[idx];
        __edges.push_back(__edge(F.nodes[0], F.nodes[1], idx));
        __edges.push_back(__edge(F.nodes[0], F.nodes[2], idx));
        __edges.push_back(__edge(F.nodes[1], F.nodes[2], idx));
    }
    // sort edges by increasing nodes
    std::sort(__edges.begin(), __edges.end());

    // Euler's formula: nodes - edges + faces = 2 (closed simpliconnected surface)
    size_t NE=nodes.size()+faces.size()-2;
    edges.clear();
    edges.reserve(NE);
    __edge &ref=__edges.front();
    size_t findex;
    edges.push_back(SurfEdge(ref.nodes));
    edges.back().set_face(findex=0, ref.face, faces[ref.face].nodes);
    faces[ref.face].set_edge(edges.size()-1, ref.nodes);
    nodes[ref.nodes[0]].AttachedEdges()++;
    nodes[ref.nodes[1]].AttachedEdges()++;
    for(size_t idx=1; idx<__edges.size(); idx++)
    {
        if(ref==__edges[idx])
        {
            edges.back().set_face(++findex, __edges[idx].face, faces[__edges[idx].face].nodes);
            faces[ref.face].set_edge(edges.size()-1, ref.nodes);
        }
        else
        {
            assert(findex==1);
            ref=__edges[idx];
            edges.push_back(SurfEdge(ref.nodes));
            edges.back().set_face(findex=0, ref.face, faces[ref.face].nodes);
            faces[ref.face].set_edge(edges.size()-1, ref.nodes);
            nodes[ref.nodes[0]].AttachedEdges()++;
            nodes[ref.nodes[1]].AttachedEdges()++;
        }
    }
    assert(edges.size()==NE);
}

bool __radius_centre_of_ball(SurfaceAlignDomain::SurfNode &X,
    std::vector<Vector3D> &pts)
{
    size_t npts=pts.size();
    std::vector<Vector3D> M;
    std::vector<double> Q;
    M.resize(npts);
    Q.resize(npts);
    for(size_t ind=0; ind<npts; ind++)
    {
        M[ind]=pts[ind]-X;
        Q[ind]=0.5*(M[ind]%(pts[ind]+X)); // % is inner product of two Vector3Ds
    }
    Matrix3D A(0.0);
    Vector3D R(0.0, 0.0, 0.0), N;
    for(size_t ind=0; ind<npts; ind++)
    {
        R+=Q[ind]*M[ind];
        for(int i=0; i<3; i++)
            for(int j=0; j<3; j++)
                A[i][j]+=M[ind][i]*M[ind][j];
    }

    if(det(A)<1e-12)
        return false;
        /*{
            // pts and X are in the same plane !?!  We are going with Plan B
            // find normal as the average of normals of pairs of edges and
            //    set center so that X-X.BallCenter is coliner with normal
            R=0.0;
            for(size_t ind=0; ind<npts; ind++)
            {
                size_t jnd=(ind+1)%npts;
                N=M[ind]*M[jnd]; // * is a cross product of two Vector3Ds
                if(N.length()<1e-12)
                    continue;
                N.normalize();
                if(ind>0&&(N%R)<0)
                    R-=N;
                else
                    R+=N;
            }
            R.normalize();
            X.BallCentre()=X+R;
            X.BallKappa()=0.0;
        }*/
    else
    {
        Matrix3D invA;
        inv(invA, A);
        X.BallCentre()=invA*R;
        double rad=Vector3D::distance_between(X, X.BallCentre());
        X.BallKappa()=1.0/rad;
        return true;
    }
}

void SurfaceAlignDomain::__compute_ball(size_t node_index)
{
    SurfNode &X=nodes[node_index];
    assert(X.AttachedNodes()>2);

    // get attached nodes
    std::vector<Vector3D> pts;
    pts.reserve(X.AttachedNodes());
    for(std::vector<SurfEdge>::iterator it=edges.begin(); it!=edges.end(); it++)
    {
        SurfEdge E= *it;
        if(E.has_node(node_index))
        {
            size_t other_node=(E.nodes[0]+E.nodes[1])-node_index;
            pts.push_back(nodes[other_node]);
            if(pts.size()==X.AttachedNodes())
                break;
        }
    }

    // compute radius and center of ball
    if(!__radius_centre_of_ball(X, pts))
    {
        Vector3D N(0, 0, 0);
        for(size_t i=0; i<faces.size(); i++)
        {
            if(faces[i].has_node(node_index))
                N+=faces[i].GetSurfNormal(nodes);
        }
        N.normalize();
        X.BallCentre()=X-N;
        X.BallKappa()=0.0;
    }
    return;
}

void SurfaceAlignDomain::compute_balls()
{
    for(size_t ind=0; ind<nodes.size(); ind++)
        __compute_ball(ind);

    write_vtk("compute_balls_before.vtk");

    std::vector<Vector3D> node_normals;
    std::set<size_t> foo_nodes;

    node_normals.resize(nodes.size(), Vector3D(0.0, 0.0, 0.0));

    Vector3D NN, FN;
    size_t nid;
    // set signs of normals
    signed int dir;
    signed int Nsign;
    for(size_t fid=0; fid<faces.size(); fid++)
    {
        SurfFace &F=faces[fid];
        FN=F.GetSurfNormal(nodes);
        for(int i=0; i<3; i++)
        {
            nid=F.nodes[i];
            SurfNode &N=nodes[nid];
            NN=N.GetBallNormal();
            Nsign=N.GetNormalSign();
            if((NN%FN)<1e-12)
                dir=-1;
            else
                dir=1;
            node_normals[nid]+=FN;
            if(N.IsNormalSignSet()&&N.GetNormalSign()!=dir)
                foo_nodes.insert(nid);
            // assert((!N.IsNormalSignSet())||N.GetNormalSign()==dir);
            N.SetNormalSign(dir);
        }
    }

    std::set<size_t>::iterator it=foo_nodes.begin();
    for(; it!=foo_nodes.end(); it++)
    {
        nid=it.operator *();
        if(nodes[nid].GetBallNormal()%node_normals[nid]<1e-12)
            nodes[nid].SetNormalSign(-1);
        else
            nodes[nid].SetNormalSign(1);
    }

    write_vtk("compute_balls_after.vtk");

    return;
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

SurfaceAlignDomain::SurfNode&
    SurfaceAlignDomain::SurfNode::operator=(const Vector3D& rhs)
{
    Vector3D::operator=(rhs);
    mKappa=0.0;
    mCenter.set(0.0, 0.0, 0.0);
    mEdgesNum=0;
    mNormalSign=0;
    return *this;
}

SurfaceAlignDomain::SurfNode&
    SurfaceAlignDomain::SurfNode::operator=(const SurfNode& rhs)
{
    Vector3D::operator=(rhs);
    mKappa=rhs.mKappa;
    mCenter=rhs.mCenter;
    mEdgesNum=rhs.mEdgesNum;
    mNormalSign=rhs.mNormalSign;
    return *this;
}

Vector3D SurfaceAlignDomain::SurfNode::GetSurfNormal() const
{
    return(mNormalSign<0)?-GetBallNormal():GetBallNormal();
}

Vector3D SurfaceAlignDomain::SurfNode::GetSurfNormal(const Vector3D &pt) const
{
    if(mKappa>0)
        return(mNormalSign<0)?-GetBallNormal(pt):GetBallNormal(pt);
    else
        return GetSurfNormal();
}

Vector3D SurfaceAlignDomain::SurfNode::GetBallNormal() const
{
    Vector3D N= *this -mCenter;
    N.normalize();
    assert(std::isfinite(N.x()));
    return N;
}

Vector3D SurfaceAlignDomain::SurfNode::GetBallNormal(const Vector3D &pt) const
{
    Vector3D N=pt-mCenter;
    N.normalize();
    if(!std::isfinite(N.x()))
        return GetBallNormal();
    else
        return N;
}

Vector3D SurfaceAlignDomain::SurfNode::GetProjection(const Vector3D &pt) const
{
    if(mKappa>0)
    {
        // let's see if point is above or below equator if *this is the North Pole
        if((pt-mCenter)%(*this-mCenter)>0)
            return mCenter+GetBallNormal(pt)/mKappa;
        else
            return mCenter-GetBallNormal(pt)/mKappa;
    }
    else
    {
        Vector3D N=GetBallNormal();
        double alpha=(pt-(*this))%N;
        return pt-alpha*N;

        // plane projection reminder: Gram-Schmidt
        // plane with point N and normal n.
        // projection of point P => C
        // the formula is: C = P - alpha n
        // where alpha = <NP, n> / <n,n>, where vector NP = P - N
    }
}


/////////////////////////////////////////////////////////////////////////////

Vector3D SurfaceAlignDomain::SurfFace::GetSurfNormal
(const std::vector<SurfNode> &sn) const
{
    Vector3D normal=// cross product; the order matters
        (sn[nodes[1]]-sn[nodes[0]]) * (sn[nodes[2]]-sn[nodes[0]]);
    normal.normalize();
    return normal;
}

/////////////////////////////////////////////////////////////////////////////

void SurfaceAlignDomain::write_vtk(const char* filename) const
{
    std::ofstream file(filename);
    if(!file)
        return;

    // Write a header
    file<<"# vtk DataFile Version 2.0\n";
    file<<"Surface mesh "<<filename<<" .\n";
    file<<"ASCII\n";
    file<<"DATASET UNSTRUCTURED_GRID\n";

    // Write vertex coordinates
    file<<"\nPOINTS "<<nodes.size()<<" float\n";
    for(size_t i=0; i<nodes.size(); i++)
        file<<nodes[i].x()<<' '<<nodes[i].y()<<' '<<nodes[i].z()<<'\n';

    file<<"\nCELLS "<<faces.size()<<' '<<4*faces.size()<<'\n';
    for(size_t i=0; i<faces.size(); i++)
        file<<3<<' '
        <<faces[i].nodes[0]<<' '
        <<faces[i].nodes[1]<<' '
        <<faces[i].nodes[2]<<'\n';

    // Write out the element types
    file<<"\nCELL_TYPES "<<faces.size()<<'\n';
    for(size_t i=0; i<faces.size(); i++)
        file<<5<<'\n';

    // Write out normals at nodes.
    file<<"\nPOINT_DATA "<<nodes.size()
        <<"\nVECTORS node_normal float\n";
    for(size_t i=0; i<nodes.size(); ++i)
    {
        Vector3D N=nodes[i].GetSurfNormal();
        file<<N.x()<<' '<<N.y()<<' '<<N.z()<<'\n';
    }

    // Write out normals at faces.
    file<<"\nCELL_DATA "<<faces.size()
        <<"\nVECTORS face_normal float\n";
    for(size_t i=0; i<faces.size(); ++i)
    {
        Vector3D N=faces[i].GetSurfNormal(nodes);
        file<<N.x()<<' '<<N.y()<<' '<<N.z()<<'\n';
    }

    // Close the file
    file<<'\n';
    file.close();

}



